#ifndef __MATH_UTILS_H__
#define __MATH_UTILS_H__

#include <iostream>
#include <vector>
#include <math.h>
#include <geometry_msgs/TransformStamped.h>

#include "base_define.h"

using namespace std;

namespace math_utils{

    /************************************
     * 角度计算
     * https://wiki.ros.org/angles
     *************************************/

    // 弧度转角度
    float Rad2Deg(float radians);
    // 角度转弧度
    float Deg2Rad(float degrees);
    // 角度转换 [-PI,PI]
    float NorMalizeAngle(float angle);
    // 角度差 b-a
    float DifferenceAngle(float a, float b);
    // 角度相加 a+b
    float AdditionAngle(float a, float b);

    // 计算两个点之间的角度
    float AnglePose2D(Pose2D a, Pose2D b);
    // 计算两点之间距离
    float DistancePose2D(Pose2D a, Pose2D b);

    // 位姿转换
    Pose2D Pose2DGlobal2Local(Pose2D curr, Pose2D global_pose);
    // 位姿转换
    Pose2D Pose2DLocal2Global(Pose2D curr, Pose2D local_pose);
    // Path 路径转换
    vector<Pose2D> PathGlobal2Local(Pose2D curr, vector<Pose2D>& points);

}; //math_utils 

#endif